

// TurtleSquare.java
import gin.and.tonic.*;
import lejos.nxt.Button;

public class Coal implements UltrasonicListener, TouchListener
{

    private DragonRobot robot = new DragonRobot();
    UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
    TouchSensor ts = new TouchSensor(SensorPort.S2);
    LightSensor rightEye = new LightSensor(SensorPort.S4);
    LightSensor leftEye = new LightSensor(SensorPort.S3);
    Motor arm = new Motor(MotorPort.A);
    int v = SharedConstants.BRIGHTDEGREE;//420;//blue river
    double r = 0.05;
    int diff = 30;
    int stopdistance = 10;

    Coal()
    {


        robot.addPart(rightEye);
        robot.addPart(leftEye);
        robot.addPart(us);
        robot.addPart(ts);
        rightEye.activate(true);
        leftEye.activate(true);

        arm.setSpeed(20);
        robot.addPart(arm);

        /*Mission coal*/
        robot.setTurtleSpeed(60);


        robot.forward(260); //get out of the base
        robot.right(90);
        robot.forward(180);


        riverToSpot();//go to the spot
        robot.backward(100);
        arm.rotateTo(133);
        arm.getMot().lock(30);
        robot.right(50);
        robot.left(62);
        arm.rotateTo(-133);
        arm.getMot().lock(30);

        spotToWall();
        robot.forward(100);


        robot.left(-30);
        robot.backward(330);
        robot.right(-30);
        robot.backward(90);
        robot.right(-60);
        robot.backward(400);




        robot.exit();

    }

    private void riverToSpot()
    {
        robot.getGear().forward();
        while (true)
        {
            int leftValue = leftEye.getValue();
            int rightValue = rightEye.getValue();
            int distance = us.getDistance();

            if (distance < stopdistance)
            {
                robot.getGear().stop();
                break;// in front of the wall
            }

            if (leftValue < v && rightValue < v)
            {
                robot.getGear().forward();
                continue;
            }
            if (leftValue < v && rightValue > v)
            {
                robot.getGear().leftArc(r);
                continue;
            }
            if (leftValue > v && rightValue < v)
            {
                robot.getGear().rightArc(r);
                continue;
            }
            if (leftValue > v && rightValue > v)
            {
                //robot.getGear().backward();
                continue;
            }
        }
    }

    private void spotToWall()
    {
        robot.getGear().backward();
        while (true)
        {
            if(ts.isPressed())
            {
                robot.getGear().stop();
                break;
            }
        }


    }

    public void far(SensorPort port, int level)
    {
    }

    public void near(SensorPort port, int level)
    {
    }

    public void pressed(SensorPort port)
    {
    }

    public void released(SensorPort port)
    {
    }

    public static void main(String[] args)
    {

        new Coal();
    }
}
